/*
 * @FileName: loop.c
 * @Author: GreyQiu qiushaogui@aikosolar.com
 * @Date: 2023-07-12 16:05:59
 * @LastEditors: GreyQiu qiushaogui@aikosolar.com
 * @LastEditTime: 2023-08-16 18:08:51
 * @Description: 
 * 
 * Copyright (c) 2023, All Rights Reserved. 
 */

#include "loop.h"

easy_pid_f32_t vout_loop; // 输出电压 PID控制
easy_pi_f32_t  iout_loop; // 输出电流 PI控制
easy_pid_f32_t vin_loop; // 输出电流 PI控制

loop_status_t loop_status;

easy_lpf_f32_t duty_flt;

uint16_t pwr_dir = 0; // 功率方向 0: 从左往右 1: 从右往做
uint16_t fsbbMode = 0;

static void vout_loop_init(void) {
    vout_loop.Kp = VOUT_PID_KP;
    vout_loop.Ki = VOUT_PID_KI * TS;
    vout_loop.Kd = VOUT_PID_KD / TS;

    vout_loop.Ymax = VLOOP_OUT_MAX;
    vout_loop.Ymin = VLOOP_OUT_MIN;

    easy_pid_init_f32(&vout_loop);

    vout_loop.ref = 5.0f;
}

static void iout_loop_init(void) {
    iout_loop.Kp = IOUT_PID_KP;
    iout_loop.Ki = IOUT_PID_KI * TS;

    iout_loop.Ymax = ILOOP_OUT_MAX;
    iout_loop.Ymin = ILOOP_OUT_MIN;

    easy_pi_init_f32(&iout_loop);

    iout_loop.ref = 0.0f;
}

static void vin_loop_init(void) {
    vin_loop.Kp = VOUT_PID_KP;
    vin_loop.Ki = VOUT_PID_KI * TS;
    vin_loop.Kd = VOUT_PID_KD / TS;

    vin_loop.Ymax = VLOOP_OUT_MAX;
    vin_loop.Ymin = VLOOP_OUT_MIN;

    easy_pid_init_f32(&vin_loop);

    vin_loop.ref = 0.0f;
}

void loop_init(void)
{
    vout_loop_init();
    iout_loop_init();
    vin_loop_init();
    loop_status.enable = DISABLE;
    loop_status.mux = 0;
    loop_status.Vset = 5.0f;
    loop_status.Iset = 7.0f;
    loop_status.VsetDischarge = 15.0f;

    easy_lpf_config_f32(&duty_flt, 1000.0f, TS); // 占空比数值滤波 截至频率250HZ
}

void loop_enable(void)
{
    loop_status.enable = ENABLE;
}

void loop_disable(void)
{
    loop_status.enable = DISABLE;
}

bool vout_loop_saturation(void)
{
    return vout_loop.Yn_1 == vout_loop.Ymax;
}

bool iout_loop_saturation(void)
{
    return iout_loop.Yn_1 == iout_loop.Ymax;
}

bool vin_loop_saturation(void)
{
    return vin_loop.Yn_1 == vin_loop.Ymax;
}

float loop_get_duty(void)
{
    return duty_flt.Yn_1;
}

void loop_task(void) 
{
    float out = duty_flt.Yn_1;
    
    if (out <= 0.9f) {
        fsbbMode = 0;
//        d[0] = out;
//        d[1] = 0.0f;
    } else if (out > 0.95f && out <= 1.0f) {
        fsbbMode = 1;
//        d[0] = out * 0.95f;
//        d[1] = 0.05f;
    } else if (out > 1.0f && out <= 1.05f) {
        fsbbMode = 2;
//        d[0] = 0.95f;
//        d[1] = 1 - (2.0f - out) * 0.95f;
    } else if (out > 1.1f){
        fsbbMode = 3;
//        d[0] = 1.0f;
//        d[1] = out - 1.0f;
    }
}
